#include "ros_web_bridge/ros_web_bridge.hpp"

#include <arpa/inet.h>
#include <sys/socket.h>
#include <zlib.h>

#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "ros_web_bridge/draco_pcl_converter.hpp"


namespace ros_web_bridge
{
RosWebBridgeNode::RosWebBridgeNode(const rclcpp::NodeOptions & options)
: Node("ros_web_bridge", options)
{
  laser_cloud_ = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
  laser_cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
    "terrain_map_ext", rclcpp::SensorDataQoS(),
    std::bind(&RosWebBridgeNode::laserCloudHandler, this, std::placeholders::_1));
}

void RosWebBridgeNode::laserCloudHandler(
  const sensor_msgs::msg::PointCloud2::ConstSharedPtr laserCloud2)
{
  laser_cloud_->clear();
  pcl::fromROSMsg(*laserCloud2, *laser_cloud_);
  // 压缩点云
  std::cout << "Compressing point cloud with " << laser_cloud_->size() << " points..." << std::endl;
  auto compressed_data = DracoPCLConverter::CompressPointCloud(laser_cloud_, 7);
  if (compressed_data.empty()) {
    std::cerr << "Failed to compress point cloud" << std::endl;
    return;
  }
  std::cout << "Compressed size: " << compressed_data.size() << " bytes" << std::endl;
  // auto bytes = serializePointCloud(laser_cloud_);
  // auto compressed_data = compressGzip(bytes);
  sendToClient(compressed_data, "192.168.1.50", 9998);
}
// 将 pcl::PointCloud<pcl::PointXYZI> 转成二进制字节数组
std::vector<uint8_t> RosWebBridgeNode::serializePointCloud(
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)
{
  std::vector<uint8_t> buffer;
  buffer.reserve(cloud->points.size() * 16);
  for (const auto & pt : cloud->points) {
    float vals[4] = {pt.x, pt.y, pt.z, pt.intensity};
    uint8_t * bytes = reinterpret_cast<uint8_t *>(vals);
    buffer.insert(buffer.end(), bytes, bytes + 16);
  }
  return buffer;
}
// 使用 zlib 进行 gzip 压缩
std::string RosWebBridgeNode::compressGzip(const std::vector<uint8_t> & input)
{
  z_stream zs{};
  deflateInit2(&zs, Z_BEST_COMPRESSION, Z_DEFLATED, 15 + 16, 8, Z_DEFAULT_STRATEGY);

  zs.next_in = const_cast<Bytef *>(input.data());
  zs.avail_in = input.size();

  int ret;
  char outbuffer[32768];
  std::string outstring;

  do {
    zs.next_out = reinterpret_cast<Bytef *>(outbuffer);
    zs.avail_out = sizeof(outbuffer);

    ret = deflate(&zs, Z_FINISH);
    outstring.append(outbuffer, sizeof(outbuffer) - zs.avail_out);
  } while (ret == Z_OK);
  std::cout << "Original size: " << input.size() << ", Compressed size: " << outstring.size()
            << std::endl;

  deflateEnd(&zs);
  return outstring;
}

void RosWebBridgeNode::sendToClient(
  std::vector<unsigned char, std::allocator<unsigned char>> compressed_data, const std::string & ip, int port)
{
  int sock = socket(AF_INET, SOCK_STREAM, 0);
  sockaddr_in server_addr{};
  server_addr.sin_family = AF_INET;
  server_addr.sin_port = htons(port);
  inet_pton(AF_INET, ip.c_str(), &server_addr.sin_addr);

  if (connect(sock, (sockaddr *)&server_addr, sizeof(server_addr)) == 0) {
    // 发送数据大小
      uint32_t data_size = compressed_data.size();
      if (send(sock, &data_size, sizeof(data_size), 0) < 0) {
          std::cerr << "Error sending data size" << std::endl;        
      }
      // 发送压缩数据
      size_t total_sent = 0;
      while (total_sent < compressed_data.size()) {
          ssize_t sent = send(sock, compressed_data.data() + total_sent, 
                              compressed_data.size() - total_sent, 0);
          if (sent < 0) {
              std::cerr << "Error sending data" << std::endl;
          }
          total_sent += sent;
      }
      std::cout << "Successfully sent " << total_sent << " bytes" << std::endl;
    // send(sock, compressed_data.data(), compressed_data.size(), 0);
  }
  close(sock);
}
}  // namespace ros_web_bridge

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(ros_web_bridge::RosWebBridgeNode)